#include "../H/DiskLoader.h"

//------------------------------------------------------------------------------
// METHOD:  DiskLoader::DiskLoader()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
DiskLoader::DiskLoader()
{
	printf("BEGIN:  DiskLoader(channel)\n");
	pClampSol      = new DoubleSolenoid (CLAMP_SOL_CHA,CLAMP_SOL_CHB);
	pRaiseMotor    = new Victor (DISK_RAISE_CH);
	pLoaderEncoder = new Encoder(LOADER_ENCODER_CHA,LOADER_ENCODER_CHB);

	pLoaderEncoder->Start();
	SmartDashboard::init();

	printf("END:    DiskLoader()\n");
}
//------------------------------------------------------------------------------
// METHOD:  DiskLoader::~DiskLoader()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
DiskLoader::~DiskLoader()
{
	// Does nothing
}
//------------------------------------------------------------------------------
// METHOD:  DiskLoader::ClampDisk()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  DiskLoader::ClampDisk()
{
	printf("BEGIN:  DiskLoader::ClampDisk()\n-----\n");

	pClampSol->Set(DoubleSolenoid::kForward);
	SmartDashboard::PutBoolean("Clamping Disk: ", true);
	printf("END:    DiskLoader::TurnOn()\n");
	return;
}
//------------------------------------------------------------------------------
// METHOD:  DiskLoader::UnClampDisk()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  DiskLoader::UnClampDisk()
{
	printf("BEGIN:  DiskLoader::UnClampDisk()\n-----\n");

	pClampSol->Set(DoubleSolenoid::kReverse);
	SmartDashboard::PutBoolean("Clamping Disk: ", false);

	printf("END:    DiskLoader::UnClampDisk()\n");
	return;
}
//------------------------------------------------------------------------------
// METHOD:  DiskLoader::MoveLoader()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  DiskLoader::MoveLoader(int position)
{

	encoderValue = pLoaderEncoder->Get();

	if(position == PickUpDisk)
	{
		if (encoderValue > target1)
			pRaiseMotor->Set(FORWARD_SPEED);
		else if (encoderValue < target1)
			pRaiseMotor->Set(REVERSE_SPEED);
		else
			pRaiseMotor->Set(STOP_MOTOR);
	}

	if(position == FeedDisk)
	{
		if (encoderValue > target2)
			pRaiseMotor->Set(FORWARD_SPEED);
		else if (encoderValue < target2)
			pRaiseMotor->Set(REVERSE_SPEED);
		else
			pRaiseMotor->Set(STOP_MOTOR);
	}

	if(position == StowLoader)
	{
		if (encoderValue > target3)
			pRaiseMotor->Set(FORWARD_SPEED);
		else if (encoderValue < target3)
			pRaiseMotor->Set(REVERSE_SPEED);
		else
			pRaiseMotor->Set(STOP_MOTOR);
	}

	SmartDashboard::PutNumber("Loader Position",position);

	printf("END:    DiskLoader::RaiseDisk()\n");
	return;
}
//------------------------------------------------------------------------------
// METHOD:  DiskLoader::SetLoaderTargets()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  DiskLoader::SetLoaderTargets(int targetPickUp, int targetLoad, int targetStow)
{
	target1 = targetPickUp;
	target2 = targetLoad;
	target3	= targetStow;

	return;
}
//------------------------------------------------------------------------------
// METHOD:  DiskLoader::ResetEncoder()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  DiskLoader::ResetEncoder()
{
	pLoaderEncoder->Reset();
	return;
}
